﻿// 基线法平差：兰交大李建章老师论文中的方法，无需求近似坐标，无需迭代平差
// 但处理隧道网时似乎精度很差？可能哪里没做好
using System;
using System.Collections.Generic;
using System.IO;
using System.Text;
using MathNet.Numerics.LinearAlgebra;
using static AdjPlane.Angle_t;

namespace AdjPlane
{
    public partial class Plane
    {
        //不迭代平差,根据观测基线造误差方程
        void GetAL(out Matrix<double> A, out Matrix<double> L)
        {
            A = Matrix<double>.Build.Dense(2 * nl, 2 * ukp_num + 2 * CZ.Count);
            L = Matrix<double>.Build.Dense(2 * nl, 1);
            int i = 0;
            int s = 0;
            foreach (KeyValuePair<string, Dictionary<string, double>> sta in obsL)
            {
                foreach (KeyValuePair<string, double> obs in sta.Value)
                {
                    double dis = obsS[sta.Key][obs.Key];
                    double angle = DMS2RD(obsL[sta.Key][obs.Key]);
                    double dx = dis * Math.Cos(angle);
                    double dy = dis * Math.Sin(angle);
                    if (!knownPoint.ContainsKey(sta.Key) && !knownPoint.ContainsKey(obs.Key))
                    {//两端都是未知点
                        int j = ukpIndex[sta.Key];
                        int k = ukpIndex[obs.Key];
                        A[i, 2 * j] = -1;
                        A[i, 2 * k] = 1;
                        A[i, 2 * ukp_num + s] = -dx;
                        A[i, 2 * ukp_num + s + 1] = dy;
                        A[i + 1, 2 * j + 1] = -1;
                        A[i + 1, 2 * k + 1] = 1;
                        A[i + 1, 2 * ukp_num + s] = -dy;
                        A[i + 1, 2 * ukp_num + s + 1] = -dx;
                        L[i, 0] = dx;
                        L[i + 1, 0] = dy;
                    }
                    else if (!knownPoint.ContainsKey(sta.Key) && knownPoint.ContainsKey(obs.Key))
                    { //观测点是已知点，测站是未知点
                        int j = ukpIndex[sta.Key];
                        A[i, 2 * j] = -1;
                        A[i, 2 * ukp_num + s] = -dx;
                        A[i, 2 * ukp_num + s + 1] = dy;
                        A[i + 1, 2 * j + 1] = -1;
                        A[i + 1, 2 * ukp_num + s] = -dy;
                        A[i + 1, 2 * ukp_num + s + 1] = -dx;
                        L[i, 0] = dx - knownPoint[obs.Key].X;
                        L[i + 1, 0] = dy - knownPoint[obs.Key].Y;
                    }
                    else if (knownPoint.ContainsKey(sta.Key) && !knownPoint.ContainsKey(obs.Key))
                    {//观测点是未知点，测站是已知点
                        int k = ukpIndex[obs.Key];
                        A[i, 2 * k] = 1;
                        A[i, 2 * ukp_num + s] = -dx;
                        A[i, 2 * ukp_num + s + 1] = dy;
                        A[i + 1, 2 * k + 1] = 1;
                        A[i + 1, 2 * ukp_num + s] = -dy;
                        A[i + 1, 2 * ukp_num + s + 1] = -dx;
                        L[i, 0] = dx + knownPoint[sta.Key].X;
                        L[i + 1, 0] = dy + knownPoint[sta.Key].Y;
                    }
                    else if (knownPoint.ContainsKey(sta.Key) && knownPoint.ContainsKey(obs.Key))
                    {  //两端都是已知点
                        A[i, 2 * ukp_num + s] = -dx;
                        A[i, 2 * ukp_num + s + 1] = dy;
                        A[i + 1, 2 * ukp_num + s] = -dy;
                        A[i + 1, 2 * ukp_num + s + 1] = -dx;
                        L[i, 0] = dx + knownPoint[sta.Key].X - knownPoint[obs.Key].X;
                        L[i + 1, 0] = dy + knownPoint[sta.Key].Y - knownPoint[obs.Key].Y;
                    }
                    i += 2;
                }
                s += 2;
            }
            L *= 1000;
        }

        void GetH1H2(out Matrix<double> H1, out Matrix<double> H2)
        {
            H1 = Matrix<double>.Build.Dense(2 * nl, nl);
            H2 = Matrix<double>.Build.Dense(2 * nl, nl);
            int i = 0;
            foreach (KeyValuePair<string, Dictionary<string, double>> sta in obsL)
            {
                foreach (KeyValuePair<string, double> obs in sta.Value)
                {
                    double dis = obsS[sta.Key][obs.Key];
                    double angle = DMS2RD(obsL[sta.Key][obs.Key]);
                    H1[2 * i, i] = -1000 * dis * Math.Sin(angle) / rou;
                    H1[2 * i + 1, i] = 1000 * dis * Math.Cos(angle) / rou;
                    H2[2 * i, i] = Math.Cos(angle);
                    H2[2 * i + 1, i] = Math.Sin(angle);
                    i++;
                }
            }
        }

        Matrix<double> GetP2(int Snum, double sigma0)
        {
            P2 = Matrix<double>.Build.Dense(Snum, Snum, 0);
            int i = 0;//观测排序
            double[] diag = new double[Snum];
            foreach (KeyValuePair<string, Dictionary<string, double>> sta in obsS)
            {
                foreach (KeyValuePair<string, double> obs in sta.Value)//照准点循环
                {
                    double S = obs.Value * (1E-3);//距离，单位是千米
                    string station = sta.Key;
                    double ms2 = Math.Pow(msa + msb * S, 2);
                    diag[i] = sigma0 * sigma0 / ms2;//距离权阵
                    i++;
                }
            }
            P2 = Matrix<double>.Build.DiagonalOfDiagonalArray(diag);
            return P2;
        }

        // 基线法平差，无需迭代，但处理隧道网时，似乎误差较大?
        // ref [8]
        void AdjustBL(string outpath)
        {
            //定权
            P1 = GetP1(nl, M0);
            P2 = GetP2(nl, M0);
            Matrix<double> Qx, Px;
            GetH1H2(out Matrix<double> H1, out Matrix<double> H2);
            Qx = H1 * P1.Inverse() * H1.Transpose() + H2 * P2.Inverse() * H2.Transpose();
            Px = Qx.Inverse();
            GetAL(out A, out L);
            var Naa = A.Transpose() * Px * A;
            var Waa = A.Transpose() * Px * L;
            var X = Naa.Inverse() * Waa;
            var Vx = A * X - L;
            foreach (KeyValuePair<string, Point2D> p in knownPoint)
                Result.Add(p.Key, p.Value);
            int i = 0;
            foreach (KeyValuePair<string, int> p in ukpIndex)
            {
                double px = X[2 * i, 0] / 1000;
                double py = X[2 * i + 1, 0] / 1000;
                Point2D poi = new Point2D(px, py);
                Result.Add(p.Key, poi);
                i++;
            }
            var T1 = H1 * H1.Transpose();
            var T2 = H2 * H2.Transpose();
            var T = T1 + T2;
            var Tinv = T.Inverse();
            var TinvA = Tinv * A;
            var C = Tinv - TinvA * (A.Transpose() * TinvA).Inverse() * TinvA.Transpose();
            double[,] Sd = { { (C*T1*C*T1).Trace(),(C*T1*C*T2).Trace()},
                {(C*T2*C*T1).Trace(),(C*T2*C*T2).Trace() }};
            double[,] Wd = { { (Vx.Transpose() * C * T1 * C * Vx)[0, 0] },
                { (Vx.Transpose() * C * T2 * C * Vx)[0, 0] } };
            var S = Matrix<double>.Build.DenseOfArray(Sd);
            var We = Matrix<double>.Build.DenseOfArray(Wd);
            var sig2 = S.Inverse() * We;

            int n = 2 * nl;
            int t = 2 * ukp_num + 2 * CZ.Count;
            double PVV = (Vx.Transpose() * Px * Vx)[0, 0];
            double sigma = Math.Sqrt(PVV / (n - t));
            var QX = (A.Transpose() * Px * A).Inverse();
            List<string> str = new List<string>
            {
                string.Format("{0,30}", "基线法平差"),
                "----------------------------------------------------"
            };
            File.WriteAllLines(outpath, str);
            OutputXY(outpath, ukpIndex);
        }

        //基线法平差
        public void BaselineAdj(ref File_t fopt, Proc_t popt)
        {
            fopt.oufile = Path.ChangeExtension(fopt.infile, "txt");//输出路径
            ReadFile(fopt.infile,popt);//读取
            AdjustBL(fopt.oufile);//平差
        }
    }
}
